#include "Interpolation.h"

/**
 * @brief 初始化坐标轴
 */
void Interpolation::clear()
{
    destroyAllWindows();
    _backgroud = Mat::zeros(720, 720, CV_8UC3);
    arrowedLine(_backgroud, Point(_backgroud.rows / 2, _backgroud.cols - 20), Point(_backgroud.rows / 2, 20),
         Scalar(255, 255, 255),1,8,0,0.01);
    arrowedLine(_backgroud, Point(20, _backgroud.cols / 2), Point(_backgroud.rows - 20, _backgroud.cols / 2),
         Scalar(255, 255, 255),1,8,0,0.01);
    imshow("Interpolation", _backgroud);
}

void Interpolation::DegSetPoint(const int &deg, const float &radius,const uint8_t pt)
{
    // begin
    int a = deg / 90;
    int b = deg % 90;
    if (pt == 0U) {
        switch (a) {
        case 0:
            _begin_pt.x = radius * cos(b * CV_PI / 180);
            _begin_pt.y = radius * sin(b * CV_PI / 180);
            break;
        case 1:
            _begin_pt.x = -radius * sin(b * CV_PI / 180);
            _begin_pt.y = radius * cos(b * CV_PI / 180);
            break;
        case 2:
            _begin_pt.x = -radius * cos(b * CV_PI / 180);
            _begin_pt.y = -radius * sin(b * CV_PI / 180);
            break;
        case 4:
            _begin_pt.x = radius * sin(b * CV_PI / 180);
            _begin_pt.y = -radius * cos(b * CV_PI / 180);
            break;
        }
    }
    else{
        switch (a) {
        case 0:
            _end_pt.x = radius * cos(b * CV_PI / 180);
            _end_pt.y = radius * sin(b * CV_PI / 180);
            break;
        case 1:
            _end_pt.x = -radius * sin(b * CV_PI / 180);
            _end_pt.y = radius * cos(b * CV_PI / 180);
            break;
        case 2:
            _end_pt.x = -radius * cos(b * CV_PI / 180);
            _end_pt.y = -radius * sin(b * CV_PI / 180);
            break;
        case 4:
            _end_pt.x = radius * sin(b * CV_PI / 180);
            _end_pt.y = -radius * cos(b * CV_PI / 180);
            break;
        }
    }
}

/**
 * @brief 坐标轴相对坐标 -> opencv图像坐标
 * @param relative_pt
 * @return
 */
Point2f Interpolation::relative2image(const Point2f &relative_pt,const uint8_t mode)
{
    Point2f image_pt(0, 0);
    if (mode == 0U) {
        int x_k = _line_pt.x >= 0 ? 1 : -1;
        int y_k = _line_pt.y >= 0 ? 1 : -1;
        image_pt.x = x_k * relative_pt.x + _bg_center.x;
        image_pt.y = -y_k * relative_pt.y + _bg_center.y;
    } else {
        int x_k = _end_pt.x >= 0 ? 1 : -1;
        int y_k = _end_pt.y >= 0 ? 1 : -1;
        image_pt.x = x_k * relative_pt.x + _bg_center.x;
        image_pt.y = -y_k * relative_pt.y + _bg_center.y;
    }

    return image_pt;
}

Interpolation::Interpolation()
{
    this->clear();
    _begin_pt = {0, 0};
    _line_pt = {0, 0};
    _bg_center = {_backgroud.cols / 2, _backgroud.rows / 2};
}

void Interpolation::line_inter()
{

    Point2f temp_pt(0,0);
    int used_steps = 0;
    int total_steps = int(abs(_line_pt.x) / _step + abs(_line_pt.y) / _step);
    while (true)
    {
        if (used_steps >= total_steps)
            break;
        Point2f last_pt = temp_pt;
        if (abs(_line_pt.x) * temp_pt.y - abs(_line_pt.y) * temp_pt.x >= 0)
        {
            temp_pt.x += _step;
        }
        else
        {
            temp_pt.y += _step;
        }
        used_steps++;
        line(_backgroud, relative2image(last_pt,0), relative2image(temp_pt,0), Scalar(0,200,235));
        imshow("Interpolation", _backgroud);
        waitKey(2);
    }
    imshow("Interpolation", _backgroud);
    if (waitKey(0) == 27)
        destroyWindow ("Interpolation");
}

void Interpolation::arc_inter()
{
    Point2f temp_pt(_begin_pt);
    int used_steps = 0;
    int total_steps = int(abs(_end_pt.x - _begin_pt.x) / _step + abs(_end_pt.y - _begin_pt.y) / _step);
    float Fm = temp_pt.x * temp_pt.x + temp_pt.y * temp_pt.y - _radius * _radius;
    while (true)
    {
        if (used_steps >= total_steps)
            break;
        Point2f last_pt = temp_pt;
           if (Fm >= 0)
            {
            Fm = Fm - 2 * temp_pt.y + 1;
            temp_pt.y-=_step;
            }
         else
            {
            Fm = Fm + 2 * temp_pt.x + 1;
            temp_pt.x+=_step;
            }
        used_steps++;
        line(_backgroud, relative2image(last_pt,1), relative2image(temp_pt,1),
             Scalar(255));
        imshow("Interpolation", _backgroud);
        waitKey(2);
    }
    imshow("Interpolation", _backgroud);
    if (waitKey(0) == 27)
        destroyWindow ("Interpolation");
}


void Interpolation::anti_arc_inter()
{
    Point2f temp_pt(_begin_pt);
    int used_steps = 0;
    int total_steps = int(abs(_end_pt.x - _begin_pt.x) / _step + abs(_end_pt.y - _begin_pt.y) / _step);
    float Fm = temp_pt.x * temp_pt.x + temp_pt.y * temp_pt.y - _radius * _radius;
    while (true)
    {
        if (used_steps >= total_steps)
            break;
        Point2f last_pt = temp_pt;
        if (Fm >= 0)
        {
            Fm = Fm - 2 * temp_pt.x + 1;
            temp_pt.x-=_step;
        }
        else
        {
            Fm = Fm + 2 * temp_pt.y + 1;
            temp_pt.y+=_step;
        }
        used_steps++;
        line(_backgroud, relative2image(last_pt,1), relative2image(temp_pt,1),
             Scalar(255));
        imshow("Interpolation", _backgroud);
        waitKey(2);
    }
    imshow("Interpolation", _backgroud);
    if (waitKey(0) == 27)
        destroyWindow ("Interpolation");
}

